#include "bhand.h"

#include <buola/app/ccmdline.h>

namespace buola { namespace robot { namespace sdh {

using namespace buola;

CCmdLineOption<double> sForce("force",L"force",0.75);
CCmdLineOption<int> sPort("device",L"index of USB device",0);

BHand::BHand()
    :   BBox("hand")
    ,   oJoints(this,"joints out",7,std::bind(&BHand::OnPullJoints,this))
    ,   oStatus(this,"status out",std::bind(&BHand::OnPullStatus,this))
    ,   mSetVelocity(this,"set velocity",std::bind(&BHand::SetVelocity,this,_1))
    ,   mSetGoal(this,"set goal",std::bind(&BHand::SetGoal,this,_1))
    ,   mSetCurrent(this,"set current",std::bind(&BHand::SetCurrent,this,_1))
    ,   mSetJointVelocity(this,"set velocity",std::bind(&BHand::SetJointVelocity,this,_1,_2))
    ,   mSetJointGoal(this,"set goal",std::bind(&BHand::SetJointGoal,this,_1,_2))
    ,   mAxis(7)
    ,   mActual(7)
{
    InitHand();
}

void BHand::InitHand()
{
    std::unique_lock<std::mutex> lLock(mMutex);

    msg_info() << "now we should initialize the hand\n";
    mSDH.reset(new cSDH(true,false,0));
    
    int lPort=cmd_line().GetValue(sPort);
    double lForce=cmd_line().GetValue(sForce);

    if(lForce<=0.0||lForce>1.0)
        throw XOptionParsing("force must be between 0 and 1");
    
    mSDH->OpenRS232( lPort, 115200, -1 );

    std::vector<double> lCurrents(7);

    msg_info() << "force set to " << lForce << "\n";

    for(int i=0;i<7;i++)
    {
        mAxis[i]=i;
        lCurrents[i]=lForce;
    }

    mSDH->SetAxisMotorCurrent(mAxis,lCurrents);
}

void BHand::OnPullJoints()
{
    std::unique_lock<std::mutex> lLock(mMutex);

    msg_info() << "pulled joints\n";
    for(int i=0;i<7;i++)
    {
        mActual[i]=mSDH->GetAxisActualAngle(i);
    }
    oJoints=mActual;
    msg_info() << "actually pulled\n";
}

void BHand::OnPullStatus()
{
    std::unique_lock<std::mutex> lLock(mMutex);

    msg_info() << "pulled status\n";
    bool lMoving=false;
    std::vector<cSDH::eAxisState> lState;
    lState=mSDH->GetAxisActualState(mAxis);
    for(int i=0;i<7;i++)
    {
        if((int)lState[i]==1)
        {
            lMoving=true;
            break;
        }
    }
    oStatus=lMoving;
    msg_info() << "actually pulled\n";
}

void BHand::SetGoal(const mat::CVecNd<7> &pJoints)
{
    std::unique_lock<std::mutex> lLock(mMutex);

    msg_info() << "in setgoal!!!\n";
    
    try
    {
        std::vector<double> lTarget(7);
        for(int i=0;i<7;i++)
        {
            lTarget[i]=pJoints[i];
            msg_info() << mAxis[i] << "," << lTarget[i]*180.0/M_PI << "\n";
        }
        mSDH->SetAxisTargetAngle(mAxis,lTarget);
        mSDH->MoveAxis(mAxis,false);
    }
    catch(...)
    {
        msg_info() << "got exception\n";
    }
}

void BHand::SetCurrent(const mat::CVecNd<7> &pCurrents)
{
    std::unique_lock<std::mutex> lLock(mMutex);

    msg_info() << "in setcurrents!!!\n";
    
    try
    {
        std::vector<double> lCurrents(7);
        for(int i=0;i<7;i++)
        {
            lCurrents[i]=pCurrents[i];
        }
        mSDH->SetAxisMotorCurrent(mAxis,lCurrents);
    }
    catch(...)
    {
        msg_info() << "got exception\n";
    }
}

/*namespace sdh*/ } /*namespace robot*/ } /*namespace buola*/ }
